#ifndef IKSOLVER_H
#define IKSOLVER_H

#include <Eigen/Dense>
#include <ode/ode.h>
#include <Eigen/StdVector>
#include <QObject>
#include "PhysicsWorld.h"
#include "Macro.h"

using namespace Eigen;
using namespace std;

class IKSolver
{
private:
	PhysicsWorld* pw;
public:
	IKSolver(PhysicsWorld* pw);
	std::vector<Joint, Eigen::aligned_allocator<Joint>> joints;
	std::vector<Link, Eigen::aligned_allocator<Link>> links;
	Vector3d endEffectorPos;
	void updateCCD(PhysicsWorld* pw);
	void updateJL(Quaternion<double>& desiredQuat, int i);
	bool computeCCD(Vector3d& endPos);
	void checkDOFRestrictions(Joint& joint, Quaternion<double>& desiredQuat);
	void reset();
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#endif